#include <string.h>
#include <stdio.h>
#include <time.h>
#include <math.h>

#include "checkNMEA.h"
#include "interpreter.h"

//You are about to make risky attempt if you try running this on 64bit.
//This program may contain Year 2038 problem.
//to use mph-feet units you need to convert within your application

unsigned long int hhmmsstt2ms(char* hhmmsstt)
{
	unsigned long int ms = 0;
	int h = 0;
	int m = 0;
	int s = 0;
	int t = 0;
	sscanf(hhmmsstt,"%2d%2d%2d%*c%3d", &h, &m, &s, &t);
    //printf("%d,%d,%d,%d\n",h,m,s,t);
	ms = h * 3600000 + m * 60000 + s * 1000 + t;
	return ms;
}
unsigned long int rtime(char* nmeastr)
{
    char hhmmsstt[11] = {'\0'};
    /*if (validate(nmeastr)) {
     ms &= 0x8000000;//set err flag
     }*/
	if((typeCode2Num(nmeastr) == 14)||(typeCode2Num(nmeastr) == 33))
	{//GGA or RMC
        sscanf(nmeastr,"%*[$]%*5s%*[,]%10s",&hhmmsstt);
        return hhmmsstt2ms(hhmmsstt);
	}
    return 0x8000000;
}

time_t dateTime(char* nmeastr)
{
    if(typeCode2Num(nmeastr) == 33)
	{//RMC
        time_t	p_time;
        struct tm record_tm;
        
        int h = 0;
        int m = 0;
        int s = 0;
        int t = 0;
        int y = 0;
        int l = 0;
        int d = 0;
        sscanf(nmeastr,
               "%*[$]%*5s%*[,]%2d%2d%2d%*c%3d%*[,]%*c%*[,]%*f%*[,]%*[NS]%*[,]%*f%*[,]%*[WE]%*[,]%*f%*[,]%*f%*[,]%2d%2d%2d", &h, &m, &s, &t,&d,&l,&y);
        //printf("%d,%d,%d,%d,%d,%d,%d\n",h,m,s,t,d,l,y);
        
        record_tm.tm_year = y + 100;
        record_tm.tm_mon = l - 1;
        record_tm.tm_mday = d;
        record_tm.tm_hour = h;
        record_tm.tm_min = m;
        record_tm.tm_sec = s;
        record_tm.tm_isdst = -1;
        
        p_time = mktime(&record_tm);
        //printf("%d\n", p_time);
        //printf("%s\n",ctime(&p_time));
        
        return p_time;
    }
    return -1;
}

double cConv(double c)
{
    double deg;
    double min;
    deg = floor(c / 100);
    min = c - (deg * 100);
    deg += min / 60;
    return deg;
}

double latitude(char* nmeastr)
{
    double l = 0;
    double deg;
    char d = 0;
    
    if(typeCode2Num(nmeastr) == 14)
	{//GGA
        sscanf(nmeastr,
               "%*[$]%*5s%*[,]%*10s%*[,]%lf%*[,]%[NS]%*s", &l, &d);
    }else if(typeCode2Num(nmeastr) == 33)
	{//RMC
        sscanf(nmeastr,
               "%*[$]%*5s%*[,]%*10s%*[,]%*c%*[,]%lf%*[,]%[NS]%*s", &l, &d);
    }
    deg = cConv(l);
    if (d == 'S') {
        deg *= -1;
    }
    return deg;
    
    //    return -1; %*[,]%f%*[,]%[WE]
}
double longitude(char* nmeastr)
{
    double l = 0;
    double deg;
    char d = 0;
    
    if(typeCode2Num(nmeastr) == 14)
	{//GGA
        sscanf(nmeastr,
               "%*[$]%*5s%*[,]%*10s%*[,]%*f%*[,]%*[NS]%*[,]%lf%*[,]%[WE]%*s", &l, &d);
    }else if(typeCode2Num(nmeastr) == 33)
	{//RMC
        sscanf(nmeastr,
               "%*[$]%*5s%*[,]%*10s%*[,]%*c%*[,]%*f%*[,]%*[NS]%*[,]%lf%*[,]%[WE]%*s", &l, &d);
    }
    deg = cConv(l);
    if (d == 'S') {
        deg *= -1;
    }
    return deg;
}

double distance(double latA, double lonA, double hA, double latB, double lonB, double hB)
{
    const double a = 6378137;
    const double f = 1/298.257222563;
    const double b = 6356752;
    const double pi = 3.141592653589793;
    double p1;
    double p2;
    double x;
    double l;
    if ((fabs(latA - latB) < 0.00001 ) && (fabs(lonA - lonB) < 0.00001 ) ) {
        return 0;
    } else {
        //deg -> rad
        latA *= pi / 180;
        latB *= pi / 180;
        lonA *= pi / 180;
        lonB *= pi / 180;
        
        p1 = atan((b/a) * tan(latA));
        p2 = atan((b/a) * tan(latB));
        
        x = acos(sin(p1) * sin(p2) + cos(p1) * cos(p2) * cos(lonA - lonB));
        l = (f / 8) * (sin(x) - x) * (pow((sin(p1) + sin(p2)),2)) / pow(sin(x),2);
        
        return a * (x + l);
    }
}

double direction (double latA, double lonA, double latB, double lonB)
{
    const double pi = 3.141592653589793;
    //deg -> rad
    latA *= pi / 180;
    latB *= pi / 180;
    lonA *= pi / 180;
    lonB *= pi / 180;
    return fmod((180 * atan2(cos(lonB) * sin(latB - latA), cos(lonA) * sin(lonB) - sin(lonA) * cos(lonB) * cos (latB - latA)) / pi) + 450, 360);
}